#include <SoftwareSerial.h>

#define left_hedge 13
#define right_hedge 12
#define left_wheel 11
#define right_wheel 10
#define Trig 8
#define Echo 7
SoftwareSerial BT(5, 6);

int left,center,right;
char BTvalue;
void forword_left(){
  analogWrite(left_wheel,80);
  analogWrite(right_wheel,185);
}

void forword_right(){
  analogWrite(left_wheel,185);
  analogWrite(right_wheel,80);
}

void forword_center(){
  analogWrite(left_wheel,165);
  analogWrite(right_wheel,175);
}

void car_stop(){
  analogWrite(left_wheel,0);
  analogWrite(right_wheel,0);
}

void parameter(){
  left=digitalRead(left_hedge);
  right=digitalRead(right_hedge);
  digitalWrite(Trig,LOW); delayMicroseconds(2);
  digitalWrite(Trig,HIGH); delayMicroseconds(10);
  digitalWrite(Trig,LOW);  
  float centerTime = float(pulseIn(Echo,HIGH)); //存储回波等待时间,
  center= (int)((centerTime * 340 )/10000)/2;   
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  BT.begin(9600);
  pinMode(left_hedge,INPUT);
  pinMode(right_hedge,INPUT);
  pinMode(Echo,INPUT);
  
  pinMode(Trig,OUTPUT);
  pinMode(left_wheel,OUTPUT);
  pinMode(right_wheel,OUTPUT);
  
  analogWrite(left_wheel,0);
  analogWrite(right_wheel,0);
}

void loop() {
  // put your main code here, to run repeatedly:
  parameter();
  
  if(BT.available()){
    BTvalue = BT.read();
    Serial.print(BTvalue);
            if(BTvalue == 'c'){
              forword_center();
            }
            if(BTvalue == 'l'){
              forword_left();
            }
            if(BTvalue == 'r'){
              forword_right();
            }   
            if(BTvalue == 's'){
              car_stop();
            }               
  }
}
